#!/usr/bin/env python

#Standard ROS imports
import roslib; roslib.load_manifest('cartographer')
import rospy

#Needed includes for this to work.
from cmvision.msg import Blobs
from cartographer.srv import map_srv
from personMap import personMap

#Just importing pi for use in the service call.
from math import pi

#Now, for a little explanation.  This is a demo, nothing more.  Do not
#use this code for any real purpose.  It was solely written to demonstrate
#the mapping function in the cartographer package.  

#This uses the blob detector from cmvision to track blobs, which are tightly controlled.
#When it detects a blob, it determines which side of the screen (left, right, or center)
#that the blob lies in, then assumes there is a person at that location.  It then updates
#the map with this false assumption.

def update_map_client(dist, angle):
    rospy.wait_for_service('mapUpdateServer')
    try:
        map_update = rospy.ServiceProxy('mapUpdateServer', map_srv)
        resp = map_update(dist, angle)
    except rospy.ServiceException, e:
        print "Service call failed: %s"%e

class blobDemo:
    def __init__(self):
        rospy.Subscriber('blobs', Blobs, self.blobCallback)
        self.demoMap = personMap()

    def blobCallback(self, data):
        oneThird = data.image_width/3
        twoThird = data.image_width - oneThird

        for blob in data.blobs:
            if blob.x <= oneThird:
                update_map_client(1.0, -pi/2.0)
            elif blob.x > oneThird and blob.x <= twoThird:
                update_map_client(1.0, 0.0)
            elif blob.x > twoThird and blob.x <= data.image_width:
                update_map_client(1.0, pi/2.0)


if __name__=='__main__':
    rospy.init_node('blobtester', anonymous = True)

    demo = blobDemo()
    rospy.spin()
